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ABSTRACT 


This report presents an algorithm that incorporates real-time data 
from two infrared cameras and computes the attitude parameters of the 
Spacecraft COntrol Laboratory Experiment (SCOLE), a laboratory 
apparatus representing an offset-feed antenna attached to the Space 
Shuttle by a flexible mast. The algorithm utilizes camera position 
information of three miniature light emitting diodes (LEDs), mounted on 
the SCOLE platform, permitting arbitrary camera placement and an on- 
line attitude extraction. The continuous nature of the algorithm allows 
identification of the placement of the two cameras with respect to some 
initial position of the three reference LEDs, followed by on-line six 
degrees of freedom attitude tracking, regardless of the attitude time 
history. The report provides a description of the algorithm in the 
camera identification mode as well as the mode of target tracking. 
Experimental data from a reduced-size SCOLE-like laboratory model, 
reflecting the performance of the camera identification and the tracking 
processes, are presented. Computer code for camera placement 
identification and SCOLE attitude tracking is listed. 
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I. INTRODUCTION 


The development of control laws for flexible structures has 
received considerable attention. To support research in this area, the 
NASA Langley Research Center has developed the Spacecraft COntrol 
Laboratory Experiment (SCOLE) (refs. 1-2). This facility provides 
researchers with a highly flexible test article, sensors, actuators, and 
digital control processing capability. The test article mimics the Space 
Shuttle with a large, flexible, offset-feed antenna attached to the 
payload bay as shown in figure 1. Much interest has been expressed by 
the research community concerning SCOLE. This is reflected in the 
technical output of five workshops conducted by the NASA Langley 
Research Center (refs. 3-7). Using SCOLE, control laws for a multi- 
input/multi-output structural dynamics system can be implemented in 
real time from any remote site that has a computer terminal and an 
asynchronous serial communications capability. Recent real-time 
experiments, representing a wide variety of applications related to 
dynamics and control of flexible structures, revealed good agreement 
with simulation results (refs. 8-9). 

Two main control objectives are stated for SCOLE (ref. 1). The first 
is a minimum-time slew through a large angle (20 degrees) reorienting 
and maintaining the "Shuttle" platform attitude to within a prescribed 
offset from an arbitrary line-of-sight reference. The second is a 
rotation of 90 degrees about the line-of-sight while preparing for 
another slew maneuver. Two research and design stages are presented 



by the SCOLE challenge. The first deals with the development of control 
laws for a mathematical model of the structure in Earth orbit, while the 
second is the design and implementation of the control laws on a 
laboratory representation of the structural dynamics system. This 
report supports, in part, the second stage of the design challenge by 
presenting an algorithm to accommodate rigid-body motion tracking. 

To detect slew maneuver and rotation about the line-of-sight, it is 
proposed to utilize the capabilities of an infrared detector system 
comprised of two cameras and three light emitting diodes (LEDs). 

Figure 2 shows the laboratory upper level with the two camera towers 
and the SCOLE LED target system. The use of such a non-inertial sensing 
device reduces the complexity inherent in any inertial sensor system 
and provides direct displacement measurements. In this report, a 
description of the test system and the algorithm for the camera self- 
calibration mode and target tracking mode are provided. Experimental 
data and test results from a reduced-size SCOLE-like laboratory model, 
shown in figures 3 and 4, are presented. The results reflect the 
performance of the camera identification and tracking processes. 
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II. THE TEST SYSTEM 


A detailed description of the SCOLE hardware and support 
software is provided in (ref. 2). It proved convenient to develop and 
test the proposed infrared detector system for SCOLE attitude extraction 
in a separate optics laboratory. To this end, a reduced size SCOLE-like 
model was designed. Figure 3 shows the model, mounted to a 
computer-controlled single degree-of-freedom precision rotary stage, as 
well as the infrared detector cameras. This setup provides a reduced 
dimension testbed that simulates the actual configuration of the SCOLE 
system, shown in figure 2. The three external LEDs, shown in figure 4, 
used in the experiment, resemble the isosceles-triangle SCOLE-LED 
configuration. This particular LED placement reduces the complexity of 
the nonlinear, rigid-body, transformation equations, permitting closed- 
form solution for attitude determination. 

Two infrared cameras were arbitrarily positioned to detect the 
three LED images during the maneuver of the SCOLE model, as shown in 
figure 5. The camera lens assembly focuses any light source within its 
field of view onto a special photodiode. The two-axis "lateral effect 
photodiode" has four electrodes on its edges. When a light spot is 
imaged onto its surface, electric current is generated, divided up among 
the four electrodes in proportion to the distance of the light spot from 
these electrodes. Denoting the detector outputs as yi , yi , z\ , and Z 2 , 
for the positive y, negative y, positive z, and negative z, respectively, 
one can determine the normalized (y,z) coordinates of the image spot on 
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the detector plane, as described in figure 6, using the following 
expressions (ref. 10): 



The resulting algebraic quantities are independent of the light intensity. 
Both cameras are interfaced to a PC-AT computer, which processes the 
sensor information and computes the rigid-body attitude angles and 
translation displacement of the SCOLE model center of mass. 
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m. A SUMMARY OF ESSENTIAL TRANSFORMATION EQUATIONS 


Assuming that light rays travel in straight lines, enter a 
distortionless camera lens system through a single point, then a 
projective relationship exists between the photographic coordinates of 
the image points and the ground coordinates of the corresponding object 
points, as illustrated in figure 7. To describe the orientation of the 
camera relative to the inertial frame, x, y, z, it suffices to state the 
orientation of the body-fixed camera coordinates, x c , y c , z c , relative to 
the SCOLE model reference frame xo, yo, zo. At this stage, the SCOLE is 
fixed to the inertial frame such that reference frames x, y, z and xo, 
yo, zo coincide. This projective relationship can be accomplished by 
means of three consecutive rotations. Since the transformation law 
deals with large rotations, emphasis is placed on consecutive rotation 
because the order in which these rotations are carried out is important. 


The following rotations are applied: 

1. Coordinate system xi, yi, zi is rotated about the zo axis over an 
angle a, positive as indicated in figure 7. The angle a is referred 

to as the heading or yaw angle. 


2. Coordinate system X2, y2* Z2 is rotated about the yi axis over an 
angle p, positive as indicated in figure 7. The angle P is referred 
to as the attitude or pitch angle. 
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3. Coordinate system x c , y c , z c is rotated about the X2 axis over an 
angle e, positive as indicated in figure 7. The angle £ is referred to 
as the bank or roll angle. 

The angles a, (3 and e are frequently referred to as Euler angles. 

Defining Vj j) as the 3x1 coordinate vector of point j projected 
onto coordinate system i, the transformation law that relates the 
camera frame to the reference frame can be written as 

v 0o) = Rv (j) (1) 

v (j) is the Newtonian coordinate vector of LED j, positioned on the 
SCOLE model as shown in figure 8. The relative camera measurement 
vector v£ 0) in equation 1 indicates the coordinates vector of LED j 
image on the camera's detector plane with respect to the computed 
camera origin 0 C , shown in figure 9. 

The ratio between sections along a straight line is known to be 
invariant under coordinate transformation. This observation facilitates 
the determination of the origin coordinates in the camera's frame, based 
on the given triangular geometry that appears in figure 10. The 
extended lines (1,2) and (0,3) intersect at point Mi 2. Similarly, lines 
(0,1) and (2,3) intersect at point M23. Since the distance ratios: 
dist(l ,Mi2)/dist(l,2), and dist(2,M23)/dist(2,3) are unaltered, camera 
image coordinates of points Mi 2 and M23 can be determined, based 
upon these ratios and camera measurement of LED-image coordinates at 
points 1, 2, and 3, as shown in figure 9. Coordinates of the intersection 
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point of lines (3, Mi 2 ) and extension of (1,M23), constitute the image 
origin Oc on the detector plane. 

The matrix R in equation 1 is the 3 x 3 transformation matrix, 
based upon the three consecutive rotations a, ( 3 , and e 


[«] = 


ca 

sa 

0 

M- 

O 

•XjD 

0 

-sp' 

[e] = 

1 

0 

0 

-sa 

ca 

0 

0 

1 

0 

0 

ce 

se 

- 0 

0 

1 . 


sp 

0 

cP. 


. 0 

-se 

ce - 


where the notations "s" and "c" abbreviate the trigonometric functions 
"sin" and "cos", respectively. In particular, the global transformation 
matrix can be expressed as 


where 


r=mMm= 


Rll R 12 R 13 
R21 R22 R23 
-R 31 R32R33- 


(2a) 


Rll = cos(P)cos(a) 

Rl2 = cos(P)sin(a) 

Rl3 = - sin(P) 

R21 = - cos(e)sin(a) + sin(e)sin(P)cos(a) 

R 22 = cos(e)cos(a) + sin(e)sin(P)sin(a) 

R23 = sin(e)cos(P) 

R31 = sin(e)sin(a) + cos(e)sin(P)cos(a) 

R 32 = - sin(e)cos(a) + cos(e)sin(P)sin(a) 

R33 = cos(e)cos(P) ( 2 b) 
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IV. CAMERA PLACEMENT IDENTIFICATION 


Utilizing the symmetry property of the LED configuration shown 
in figure 10 and introducing the camera scale factor X as the unknown 
ratio between the dimensions of the SCOLE model and the camera 
image, the following relationships for LEDs 1, 2, and 3 are proper: 

v< 1 >=[C0 0] T , v< 10 > = v< ! > - v£ 0) 

v c 10 ^ = — RvO) = -t-[Rn r 2 i R 3 1 7 
X X 

V< 2 > = [-h 0 Of, V< 20 > = V< 2 > - V<°> 

v (20) = 1_ r v ( 2) = J-[-Riih+Rj2b -R2ih+R22b 
X X 

v< 3 > = [-h -b 0] T , v< 30 > = v< 3 > - v<°> 

vOO) = 1 r v ( 3) = L[.R llh .R 12 b -R 21 h-R 22 b 
XX 

where L, h, and b are the SCOLE LED placement measurements, as 
indicated in figure 10. Introducing the relative measurement vector 
components v£°) = [x£°> y 0») z W)f and substituting equations 2b into 3b 
and 3c, the following expressions can be derived 

- cos(e)sin(a) + sin(e)sin(P)cos(a) = - X - y ^ * + y ° 30> (4a) 

2h v ' 

_(20) . _(30) 

sin(e)sin(a) + cos(e)sin(P)cos(a) = - X =* — — ^ — (4b) 


-R3ih+R32bJ r 


(3b) 


-R 3 ih-R32b] T 


(3c) 
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(4c) 


( 20 ) . ( 30 ) 

cos(e)cos(a) + sin(e)sin(P)sin(a) = X ^ ^ — 

2b 

MO) _ ^ 30 ) 

- sin(e)cos(a) + cos(e)sin(|3)sin(a) = X — £ — (4d) 

2b 

An explicit expression for e can be obtained by eliminating a and (3 
from equations 4, yielding the (e,^) polynomial 

F(e,A,) = A cos 2 (e) + B cos(e)sin(e) + C sin 2 (e) - 1 = 0 (^ a ) 


where 



are constants, based on the SCOLE configuration parameters h, b, camera 
measurements yc 0) , z c 0) , (j = 2,3), and the unknown camera constant 
scale factor X. Substitution of expressions 2b for R mn (m,n = 1,2,3) in 
equations 5b and then in equation 5a, must satisfy the identity 
F(e,A.) = 0. Moreover, differentiating equation 5a with respect to e and 
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using equations 2b and 5b again, reveals 9F(e,X,)/9e = 0. These 
observations constitute the method for the determination of the camera 
roll angle e and scale factor X. A typical solution to equation 5 is 
displayed in figure 11. Camera yaw angle a and pitch angle P are 
obtained from equations 4 


a = sin' 1 [si cos(e) - S 2 sin(e)] 


( 6 ) 


P 


= sin 


C 2 + sin(e)cos(a) 
cos(e)sin(a) 


(7) 


where 

51 = (y? 0) + y< 30> ) {- s gn [v£ 2) (l )]}/(2h) 

52 = (z? 0) + 4 30) ) {- sgn [v< 2 >(l)]}/(2h) 


c 2 = (z< 20 > - z< 30 >)/(2b) 


sgn [vq 2) (1)J is the standard sign function with the first entry of vector 
( 2 ) 

v Q from equation 3b as an argument. 


The method of determination of the orientation angles a, p, and e, 
for both cameras described herein, permits arbitrary camera placement. 
The three-LED system mounted on the initially fixed SCOLE model 
during this process constitutes the reference Newtonian frame for the 
attitude tracking mode. 
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V. SCOLE ATTITUDE TRACKING 


The SCOLE tracking process is related to the identification of the 
three displacement components, x(0), y(0), z(°), of the SCOLE center of 
mass, and the yaw, pitch, and roll attitude angles \| /, fl, <j>, of its platform, 
respectively, as shown in figure 12, and can be regarded as the 
reversed version of the camera placement identification algorithm. 


Substitution of the identified camera orientation angles ai, Pi, 
and ei, (i=l,2), yields the transformation relations between each 
camera and the SCOLE model 


where 





= R2V 


(j) 

0 


v <i) = v <i> - v (°> 


(8a, b) 

(8c) 


In these equations Ri and R 2 are the constant coefficient 
transformation matrices in the form of equation 2, evaluated for camera 
1 and 2 angles ai , Pi , ei , and a 2 , P2 , e 2 , respectively. In a similar 
manner v^ 0 ^ and v^^ indicate the relative coordinate vector of LED j 
image onto camera 1 and 2 body frame with respect to the 
instantaneous computed image of the SCOLE model origin 0. v(i) and 
v (0) in equation 8c are the Newtonian coordinate vector of LED j and 
the SCOLE model origin 0, respectively. Vq^ is the LED j coordinate 
vector in the translated frame xq ,yo ,zq. Eliminating Vq^ from 



equations 8a, b, yields the transformation relation between the two 
cameras 


r (j0)_ 


c 1 


= R v 


(jO) 
c2 ’ 


R* = R1R2 , 


j= 0,1,2, 3 


( 9 ) 


j=0 represents the displacement of the image instantaneous center of 
mass with respect to the computed image fixed origin. Expressing v^ ( 

(i=l,2) in terms of the Cartesian components v^=[x^ z^ 0 ^, 

and v? 2 °> = y^ 2 } z^ 2 ) ] T , an explicit expression for x^ and x^ 2 0) 

can be derived 


Axj = bj, j = 0,1, 2,3 

where 


(10) 


A = 


1 

0 

0 


-Rn 
-R*21 ’ 

-R31 - 




bj = 


R 

1 

R 

v 

L R 32 


12 


22 


Rl3 

R23 

R 33 



0 

■ y c T • 

7 (j0) 
l^cl J 


Because this set of three equations contains only two unknown 
quantities, x^ 0) and x^ , it is said to be overdetermined and any 
subset of two equations may, in principle, be solved to recover x^ 
and x^ 2 \ In practice, however, the detector plane image coordinates 
y% 0) , are known within some error bound. Consequently, 

solutions to the various subsets of two equations will yield somewhat 
different results. In such a case, a preferable practice is to employ the 
method of Least Squares (LS) to extract from the entire set of three 
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equations a reduced set of two equations leading to results of greatest 
possible accuracy. Using standard LS approximation, the recovered 
unknown vectors Xj are obtained by 

xj = (A T A)' 1 A T bj , j = 0,1, 2,3 (li) 

and the amount of error in this approximate solution is indicated by 

Hell? = bj [I - A(A t A)- 1 A t ] bj (12) 

With the recovered Xj vectors, the camera information is complete and 
available for the determination of the global coordinates of the three 
SCOLE LEDs. This can be accomplished by using the inverse 
transformation 


y® y ( cf w « HA3 03) 


Subsequently, the angular motion parameters, Y> an< ^ 0 are 
calculated, based on the above information and the geometry of figure 
12 : 
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(16) 


<(> = sin 


z o 2) + h sgn [ v q 2) ( 1 > sin(fl) 
b cos(fl) 


where superscripts in equations 14 through 16 indicate LED index 
number and the subscripts are associated with the related coordinate 
system. Similarly, the displacement vector of the SCOLE center of mass 
is obtained by the inverse transformation 


[ X (0) y(0) 


E «»r = r; '[x (00) 


,( 00 ) ( 00 ) 


Cl 


'Cl 


(00) 1 

ci J ’ 


for i=l or i=2 


(17) 


where x (0 \ y (0 \ z<°) f are absolute displacement components of the 
SCOLE center of mass and x ( c ° 0) , y^ 0) , z ( c ° 0) , represent the camera frame 
displacement of the instantaneous center-of-mass image with respect to 
the camera-fixed origin. 


The continuous nature of the algorithm permits instantaneous 
tracking of the SCOLE attitude regardless of the trajectories time history. 
Using tracking techniques based on classical methods (ref. 11), current 
attitude estimate depends on the preceding estimate. With this 
approach, in a case when the target signal is temporarily lost or 
corrupted during operation, it will be most difficult, if not impossible, to 
relocate the model without process interruption. 
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VI. NUMERICAL RESULTS AND DISCUSSION 


Rigid body attitude maneuver tracking was tested on a reduced- 
size SCOLE-like laboratory model using the methods and equipment 
previously described. For this work, due to hardware limitations, the 
model was attached at its center of mass to a single degree-of-freedom, 
precision, rotary stage, as shown in figure 4, resulting in rotation about 
the yaw axis only. The rotary stage controller was able to provide 
motion with a resolution of 0.001 degrees. This laboratory setup 
provides a dynamics-free environment, where the sole testing of 
software and sensor hardware is performed. The experiment consists of 
two phases. During phase I, both cameras were oriented such that all 
of the light sources appear within the field of view of each camera, but 
otherwise arbitrarily. The SCOLE model was oriented to a desired initial 
position where the SCOLE body frame at that instant constitutes the 
system inertial reference. Camera identification, based upon the 
procedure previously described, yields camera orientation angles with 
respect to Newtonian frame, as well as camera scale factors. Sensor 
measurements, summarized in Table I, are consistent with results based 
on external measurement. Figure 13 displays a typical scaled LED 
image on the detector plane in the camera identification mode. 

Phase II of the experiment deals with attitude tracking of the 
SCOLE model. During this phase, the rotary stage was turned and LED 
position measurements were detected by both cameras simultaneously 
at every 5-degree increment. Figure 14 displays camera scaled images 
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at various model orientations. Figure 15 depicts sensor outputs for a 
rotation range of 50 degrees, where each measurement is based on the 
average of six samples. Due to the system constraints, yaw was the only 
attitude change expected. However, as shown in figure 15, non-zero 
pitch and roll motion were detected as well. This is due to 
misalignment between the LED-plane normal vector and the rotary 
stage axis of rotation. The same offset also affects the yaw 
measurements resulting in deviation from the expected linear 
characteristics. Defining linearity error as the difference between a 
data point and the associated linear least-squares approximation of the 
entire data set, the average error over the range of 50 degrees was 
0.56 degrees. Figure 16 displays the yaw error characteristics, spanned 
over the experimental range. Although motion of the model center of 
mass was constrained by the experimental setup, non-zero 
displacement components were detected by both cameras, as shown in 
figures 17 and 18. The first figure exhibits a camera scaled view of the 
image center-of-mass motion while the second provides a description of 
the motion components in an Newtonian frame. These unexpected 
readings are due primarily to detector non-linearities. 

Error reduction can be achieved through system upgrading. This 
amounts to sensor random-noise level reduction by replacement of the 
existing LEDs with more powerful ones along with amplifier gain 
reduction. Uncertainties in measurements of the LED configuration on 
the SCOLE model, and possible offset between the location of the camera 
image computed origin and the axis of rotation of the rotary stage, 
contribute to error accumulation. Another error source is encountered 
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with sensor distortion. Because of geometrical considerations involving 
detector electrode placement, the most linear region of the detector is 
the 25 -percent zone around its center. However, for maximum 
linearity, the center 6.25-percent zone of the detector should be used 
(ref. 10). Minimum linearity zone is observed at the corner of the 
detector; however, as the image spot moves towards the center, 
linearity is improved. Applications needing the maximum accuracy 
require distortion mapping of the detector surface and lens calibration. 
Such procedures were presented in (ref. 12), where a distortion 
correction was determined by fitting the sensor data to scanner data, 
using least-squares approximation. Correction of misalignment 
associated with the scanning operations and a table lookup procedure to 
map the detector surface can improve image distortion correction, and, 
therefore, increase camera output accuracy. 


VII. CONCLUDING REMARKS 


An attitude tracking algorithm, utilizing position information of 
three selected fixed points on the SCOLE platform, was introduced. The 
data acquisition system utilizes the capabilities of an infrared detector 
system comprised of two cameras, fixed to a Newtonian frame, and 
three light emitting diodes, attached to three selected points on SCOLE. 
Both cameras are interfaced to a PC-AT computer which processes 
sensor information and computes the SCOLE rigid-body attitude angles 
and displacement vector of its center of mass. The experiment consists 
of two phases. During phase I, both cameras are operating in the self- 
calibration mode, where orientation angles for each camera body frame 
with respect to the Newtonian frame are determined. In addition, the 
scale factor for each camera, defined as the ratio between the physical 
dimensions of the object and its image on the detector surface, is 
computed. During phase II, both cameras are operating in the tracking 
mode, where position information of the three LEDs is acquired and 
instantaneous yaw, pitch, and roll angles, as well as centroid 
displacement components, are determined. Yaw maneuver tests, 
performed on a reduced-size SCOLE-like laboratory model, revealed an 
average linearity error of 0.56 degrees over the range of 50 degrees. 
The results obtained here agree with other results obtained when a 
nonlinear tracking method was tested. Momentary loss of signal or 
signal corruption does not affect the tracking process. The continuous 
nature of the algorithm presented here permits instantaneous tracking 
of the SCOLE attitude parameters regardless of the attitude time history. 
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System upgrading can contribute to error reduction. More powerful 
LEDs along with amplifier gain reduction will improve the signal-to- 
noise ratio. Improving sensor distortion mapping techniques will yield 
more accurate sensor output and, therefore, also more accurate attitude 
measurements. 
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Table I.- Cameras orientation angles a, P, and e 
and scale factors X 



a 0 

P° 

e° 


CAMERA 1 

89.58 

-43.80 

0.12 

95.84 

CAMERA 2 

-87.58 

-39.07 

-1.52 

102.54 
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Figure 2.- SCOLE laboratory upper level view with two 

camera towers 
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Figure 3.- Laboratory setup of infrared detector cameras and the SCOLE 

reduced-size model 
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Figure 4.- SCOLE reduced-size model with LED configuration 
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Figure 5 - Camera placement angles in Newtonian coordinates system 
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z 2 


Figure 6 - True position (y,z) of a light spot on detector plane 
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Figure 7.- Coordinate system transformation 


t x 



Figure 8 - LED placement configuration on SCOLE 
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Figure 9.- LED image location and origin determination geometry on 

detector plane 
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Figure 10.- LED triang 
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configuration on SCOLE 





Figure 12.- SCOLE attitude angle extraction 
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Figure 13.- A typical scaled-image in the camera placement mode 
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Figure 14.- Camera scaled-image at various model orientations 
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Figure 17.- Camera scaled view of center-of-mass motion 





APPENDIX 


PC-MATLAB COMPUTER CODE FOR CAMERA PLACEMENT 
IDENTIFICATION AND SCOLE ATTITUDE TRACKING 


PC-MATLAB computer language (ref. 13) was used for the coding 
of the two-phase algorithm. The code consists of two main programs 
and thirteen sub-programs ".m" files. The first main program, 

"camid.m", provides a procedure for identification of the two camera 
attitude parameters with respect to a Newtonian frame. The second, 
"scolid.m", computes the instantaneous attitude parameters of the SCOLE 
platform with respect to that Newtonian frame. The sub-programs 
listed in the sequel, appear according to their first occurrence in the 
main calling program. The task of a program or a sub-program is 
explained under the NAME statement in each code. 
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% NAME: camid.m (main program) 


%»»»»»»»» CAMERA IDENTMCAHON «««««««« 


% Program computes camera placement parameters with 
% respect to a Newtonian reference frame 
% 

clear 

% 

% Import on-line camera data from 8751 micro processor 

% 

[xl ,x2,y 1 ,y2]=flyy % sub-program 

format compact 

scolpar % sub-program 

preorigin % sub-program 

% 

% Determine CAM1 ABC coefficients 


% 

yl=-yi ; 

y2=-y2; 

% 

% Distortion corrector for CAM1 

% 

[xl 1 ,y 1 l]=cammapl(xl,yl); 
xl=xl 1; 
yl=yl 1; 
cla 

sfl0=50; 

lecho Please wait . . . 

ycl=xl(l); 

zcl=y 1(1); 

yc2=xl(2); 

zc2=yl(2); 

yc3=xl(3); 

zc3=yl(3); 

origin 

yc01=yc0; 

zc01=zc0; 

sfO=sflO; 

rel 

abc 

scale 


% sub-program 


% sub-program 


% sub-program 
% sub-program 
% sub-program 
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epsl=xmax; 

alphal=alpha; 

betal=beta; 

sfl=sf; 

A1=A;B1=B;C1=C; 

% 

sfO=sfl; 

rel 

cam=l ; 
relplot 
pause 
% 

% Determine CAM2 ABC coefficients 

% 

xl00=xl; 
y 100=y 1 ; 
xl=x2; 

y i =y 2; 

% 

% Distortion corrector for CAM2 

% 

[x22,y22]=cammap2(xl ,y 1 ); 

x2=x22; 

y2=y22; 

xl=xl00; 

y l=y 100; 

sf20=50; 

ycl=x2(l); 

zcl=y2(l); 

yc2=x2(2); 

zc2=y2(2); 

yc3=x2(3); 

zc3=y2(3); 

origin 

yc02=yc0; 

zc02=zc0; 

sf0=sf20; 

rel 

abc 

scale 

cla 

clg 


% sub-program 
% sub-program 


% sub-program 


% sub-program 


% sub-program 
% sub-program 
% sub-program 
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eps2=xmax; 

alpha2=alpha; 

beta2=beta; 

sf2=sf; 

A2=A;B2=B;C2=C; 

% 

sf0=sf2; 

rel 

cam=2; 

relplot 

pause 

% 

!echo CAMERA 1 IDENTIFICATION 
!echo *********************** 

!echo . 

sprintf(' ALPHA- 1 = %g',alphal/dpr) 

sprintf('BETA-l = %g',betal/dpr) 
sprintf('EPSILON- 1 = %g’,epsl/dpr) 
sprintfCSCALE FACTOR- 1 = %g\sfl) 

% 

!echo . 

!echo . 

!echo CAMERA 2 IDENTIFICATION 
!echo *********************** 

!echo . 

sprintfC ALPHA-2 = %g',alpha2/dpr) 
sprintf('BETA-2 = %g\beta2/dpr) 
sprintf('EPSILON-2 = %g’,eps2/dpr) 
sprintfCSCALE FACTOR-2 = %g’,sf2) 


% sub-program 
% sub-program 
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% NAME: scolid.m (main program) 

% ==== = = =: , = — 

% »»»»»»»»»»»> SCXXE TRACKING <<<<<<<<<<<<<<<<<<<< 


% Program identifies the attitude parameters of SCOLE with 
% respect to the Newtonian frame established by the CAMID 
% program 
% 

alpha=alphal; 
beta=betal ; 
eps=epsl; 

tmatrix % sub-program 

R1=R; 

alpha=alpha2; 

beta=beta2; 

eps=eps2; 

tmatrix % sub-program 

R2=R; 

% 

% Construct matrices for the recovery of the missing 
% cameras components xcl, xc2 
% 

INVR2=inv(R2); 

R=R1*INVR2; 

Ml=[l -R(1,1);0 -R(2,l)]; 

INVM 1 =inv(M 1 ); 

Ml 1=INVM1 *R(1 :2,2:3); 

% 

% Import on-line camera data from 8751 micro processor 

% 

[xl,x2,yl,y2]=flyy % sub-program 

% 
cla 

lecho Please wait . . . 
format compact 
% 

yl=-yl; 

y2=-y2; 

% 

% Distortion corrector for CAM1 

% 

[xll,yl l]=cammapl(xl,yl); % sub-program 

xl=xl 1; 


46 


lillijllUil 



yl=yl 1; 

ycl=xl(l); 

zcl=yl(l); 

yc2=xl(2); 

zc2=yl(2); 

yc3=xl(3); 

zc3=yl(3); 

sfO=sfl; 

yc0=yc01; 

zc0=zc01; 

re l % sub-program 

vl=[0 0 0 

yclO yc20 yc30 
zclO zc20 zc30]; 

% 

% Distortion corrector for CAM2 
% 

xl00=xl; 

yl00=yl; 

xl=x2; 

yl=y2; 

[x22,y22]=cammap2(xl,yl); % sub-program 

x2=x22; 

y2=y22; 

xl=xl00; 

y l=yl00; 

ycl=x2(l); 

zcl=y2(l); 

yc2=x2(2); 

zc2=y2(2); 

yc3=x2(3); 

zc3=y2(3); 

sf0=sf2; 

yc0=yc02; 

zc0=zc02; 

re l % sub-program 

v2=[0 0 0 

yclO yc20 yc30 
zclO zc20 zc30]; 

xcleast % sub-program 

vl(l,l :3)=vl (1,1 :3)+xc( 1,1:3); 
v2(l,l:3)=v2(l,l:3)+xc(2,l:3); 
vreal=INVR2*v2; 
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cla 

sprintfC'SCOLE CURRENT ATTITUDE:’) 
sprintf('***********************’) 

angles % sub-program 


% NAME: flyy.m (sub-program) 

% Routine activates the 8751 micro processor for 
% camera data acquisition and storage in data arrays 

% 

function[x 1 ,x2,y 1 ,y2]=flyy() 

data=camera(3); 

for x=l:6; 

xa(x)=data((x-l)*8+l); 
xb(x)=data((x- 1 ) *8+2); 
ya(x)=data((x- 1 )*8+3); 
yb(x)=data((x- 1 )*8+4); 
xa2(x)=data((x-l)*8+5); 
xb2(x)=data((x-l)*8+6); 
ya2(x)=data((x-l)*8+7); 
yb2(x)=data((x-l)*8+8); 
end; 

xl=(xa-xb)./(xa+xb);%-.14187; 
x2=(xa2-xb2)./(xa2+xb2);%+.0702; 
yl=(ya-yb)./(ya+yb);%+.0287; 
y2=(ya2-yb2)./(ya2+yb2);%-. 07604; 


% NAME: scolpar.m (sub-program) 

% SCOLE’s Hardware parameters input 

% 

1=7.0; 

h=(3+3/16); 
b=4.5; 
v0=[l -h -h 
0 b -b 
0 0 0 ]; 

% 

dpr=pi/l 80; 
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% NAME: preorigin. m (sub-program) 

% Routine computes parameters related to the 
% LEDs placement geometry 
% 

% Subscript "s" means SCOLE, "c" means CAMERA 

% 

% Coordinates on SCOLE: 

% 

xsl=vO(l,l); 

ysl=v0(2,l); 

zsl=v0(3,l); 

xs2=v0(l,2); 

ys2=v0(2,2); 

zs2=v0(3,2); 

xs3=vO(l,3); 

ys3=v0(2,3); 

zs3=v0(3,3); 

% 

% SCOLE platform triangular geometry 

% 

% Ml 2 coordinates: 

% 

xsl2=-xsl *xs2/(xsl-2*xs2); 
ysl2=ys2*xsl/(xsl-2*xs2); 

% 

% M23 coordinates: 

% 

xs23=xs2; 

ys23=0; 

barlM12=sqrt((xsl-xsl2) A 2+(ysl-ysl2) A 2); 
bar 1 2=sqrt((xs 1 -xs2) A 2+(y s 1 -y s2) A 2); 
ratiol2=barlM12/barl2; 
ratio23=0.5; 


% NAME: cammapl.m (sub-program) 

% CAM1 detector map for distortion correction 

% 

function [xl l,yl l]=cammapl(xl,yl) 
for x=l:6; 

xp(x)=-.0137+.1702*xl(x)-.0003*yl(x)+.0018*(xl(x)*yl(x)); 
xpp(x)=xp(x)-.01 13*xl(x) A 2-.0052*yl(x) A 2; 
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xlp(x)=xpp(x)-.0019*xl(x) A 2*yl(x)+.0212*xl(x)*yl(x) A 2; 
xl l(x)=xlp(x)+.0884*xl(x) A 3-.0036*yl(x) A 3; 
yp(x)=.0035-.0006*xl(x)+.1721*yl(x)-.0019*xl(x)*yl(x); 
ypp(x)=yp(x)+.0085*x 1 (x) A 2+.0006*y 1 (x) A 2; 
ylp(x)=ypp(x)+.0423*xl(x) A 2*yl(x)-.003*xl(x)*yl(x) A 2; 
yl l(x)=ylp(x)+.0107*xl(x) A 3+.0678*yl(x) A 3; 
end; 


% NAME: origin. m (sub-program) 

% Routine computes the coordinates of SCOLE's origin 
% in the camera frame. 

% 

% CAMERA frame triangular geometry 

% 

ycl 2=yc 1 +(yc2-yc l)*ratiol 2; 
zcl2=zcl+(zc2-zcl)*ratiol2; 
yc23=yc2+(yc3-yc2)*ratio23; 
zc23=zc2+(zc3-zc2)*ratio23; 

% 

% Equation of line 3 -Ml 2 

% 

ml2=(zc3-zcl2)/(yc3-ycl2); 

bl2=(yc3*zcl2-zc3*ycl2)/(yc3-ycl2); 

% 

% Equation of line 1-M23 

% 

m23=(zcl -zc23)/(ycl-yc23); 
b23=(ycl*zc23-zcl*yc23)/(ycl-yc23); 

% 

% Coordinates of origin in camera's frame 

% 

zc0=(ml2*b23-m23*bl2)/(ml2-m23); 

yc0=-(bl2-b23)/(ml2-m23); 
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% NAME: rel.m (sub-program) 

% Routine computes the projections of the position 
% vectors of SCOLE's LEDs 

% 

yclO=(ycl-ycO)*sfO; 

zclO=(zcl-zcO)*sfO; 

yc20=(yc2-yc0)*sf0; 

zc20=(zc2-zc0)*sf0; 

yc30=(yc3-yc0)*sf0; 

zc30=(zc3-zc0)*sf0; 


% NAME: abc.m (sub_program) 

% Routine computes the polynomial coefficients of equation (5a). 

% 

A=((yc20+yc30)/2/h) A 2+((yc20-yc30)/2/b) A 2; 

B=(yc20+yc30)*(zc20+zc30)/2/h/h+(yc20-yc30)*(zc20-zc30)/2/b/b; 

C=((zc20+zc30)/2/h) A 2+((zc20-zc30)/2/b) A 2; 


% FILE NAME: scale.m (sub-program) 

% Routine computes camera scale factor and the 
% camera placement parameters, the angles ALPHA, BETA, 
% and EPSILON 

% 

dpr=pi/l 80; 

A0=A;B0=B ;C0=C; 

maxold=0; 

delta=l; 

i0=31; 

ifin=61; 

xbegin=0; 

convrg=0. 0000001 ; 
convdel=0.001; %degrees 
flag=0; 
sf=sf0; 
for j=l : 1000 
% 

clear xx yy 
for i=l:ifin 

x=xbegin+(i-i0)*delta; 
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xx(i)=x; 

x=x*dpr; 

yy(i)=A*(cos(x)) A 2-B*sin(x)*cos(x)+C*(sin(x)) A 2-l; 

x=x/dpr; 

end 

% 

[maxyy,ii]=max(yy); 

xmax=xx(ii); 

% 

xbegin=xmax-2*delta; 

delta=delta/2; 

i0=0; 

ifin=8; 

if delta<=convdel&abs(maxyy)<=convrg 
break 
end 

% 

maxoId=maxyy; 

sf=sf * sqrt( 1/(1 +maxyy )); 

sfr=sf/sfO; 

sfrsq=sfr A 2; 

A=AO*sfrsq; B=BO*sfrsq; C=CO*sfrsq; 
end 

% 

sl=(yc20+yc30)/2/h*(-sign(v0(l,2))); 

s2-(zc23+zc30)/2/n*(-sign(v0(l,2))); 

cl=(yc20-yc30)/2/b; 

c2=(zc20-zc30)/2/b; 

sl=sl*sfr;s2=s2*sfr;cl=cl*sfr;c2=c2*sfr, 

xmax=xmax*dpr; 

% 

alpha=(asin(s 1 *cos(xmax)-s2*sin(xmax))); 
beta=asin((c2+sin(xmax)*cos(alpha))/cos(xmax)/sin(alpha)); 


% NAME: relplot.m (sub-program) 

% Routine plots the relative coordinates of the LEDs 
% image on the detector plane 
% 

yyrel=[yclO yc20 yc30]; 
zzrel=[zclO zc20 zc30]; 
yy=[ycl yc2 yc3]; 
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zz=[zcl zc2 zc3]; 
axis('square') 
axis([-8,8,-8,8]) 
plot(yyrel,zzrel,'o') 
hold 
% 

yy=yy*sfO; 

zz=zz*sfO; 

ycO=ycO*sfO; 

zcO=zcO*sfO; 

% 

plot(yy,zz,'+',ycO,zcO,'*') 
num=[T ’2' ’3']; 
setstr(num) 
for i=l:3 

text(yyrel(i),zzrel(i),num(i)) 

end 

if (cam==l);title('CAMERA 1 ACTUAL DATA');end 
if (cam==2);title('CAMERA 2 ACTUAL DATA');end 
grid 
hold off 


% NAME: cammap2.m 

% CAM2 detector map for distortion correction 

% 

function [xl 1 ,y 1 l]=cammap2(xl,yl) 
for x=l:6; 

xp(x)=.021 5+. 1463 *xl(x)-.0051*yl(x)-. 0021 *(xl(x)*yl(x)); 
xpp(x)=xp(x)+.0017*xl(x) A 2+.0133*yl(x) A 2; 
xlp(x)=xpp(x)-.0005*xl(x) A 2*yl(x)+.0074*xl(x)*yl(x) A 2; 
xl l(x)=xlp(x)+.076*xl(x) A 3-.0074*yl(x) A 3; 
yp(x)=-.0193+.0027*xl(x)+.1881*yl(x)+.0033*xl(x)*yl(x); 
ypp(x)=yp(x)-.0166*xl(x) A 2+.0066*yl(x) A 2; 
ylp(x)=ypp(x)+.0327*xl(x) A 2*yl(x)-.0088*xl(x)*yl(x) A 2; 
yl l(x)=ylp(x)+.0024*xl(x) A 3+.0711*yl(x) A 3; 
end; 
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% NAME: tmatrix.m (sub-program) 

% Routine computes the R=EPS*BETA*ALPHA 
% transformation matrix 

% 

salpha=sin(alpha); 

calpha=cos(alpha); 

sbeta=sin(beta); 

cbeta=cos(beta); 

seps=sin(eps); 

ceps=cos(eps); 

ALPHA=[calpha salpha 0;-salpha calpha 0;0 0 1]; 
BETA=[cbeta 0 -sbeta;0 1 0; sbeta 0 cbeta]; 
EPS=[1 0 0;0 ceps seps;0 -seps ceps]; 
R=EPS*BETA*ALPHA; 


% NAME: xcleast.m (sub-program) 

% Routine computes the xcl, xc2 components via 
% least squares approximation 
% 

D=[l -R(l,l) 

0 -R(2,l) 

0 -R(3,l)]; 

DTR=D’; 

F=inv(DTR*D); 

E=[R(1,2) R(l,3) 

R(2,2) R(2,3) 

R(3,2) R(3,3)l; 
subv2=v2(2:3,l :3); 
subvl=[0 0 0 

vl(2,l) v 1(2,2) v 1(2,3) 
v 1(3,1) v 1(3,2) v 1(3,3)]; 
xc=F*DTR*(E*subv2-subvl); 


% FILE NAME: angles.m (sub-program) 

% routine computes the attitude parameters based upon 
% the (xi,yi,zi), i= 1,2,3 coordinates 
% 

x=vreal(l,l:3); 

y=vreal(2,l:3); 
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z=vreal(3,l:3); 

% 

% Compute the attitude angles PSI, THETA, IT 

% 

psi=atan(y(l)/x(l)); 

theta=atan(-z(l)/sqrt(x(l) A 2+y(l) A 2)); 

hh=h*(-sign(vO( 1 ,2))); 

xbar=x(2)+hh*cos(theta)*cos(psi); 

ybar=y(2)+hh*cos(theta)*sin(psi); 

zbar=z(2)-hh*sin(theta); 

bl =sqrt(xbar A 2+ybar A 2+zbar A 2); 

phi=asin((z(2)-hh*sin(theta))/cos(theta)/bl); 

sprintf('YAW PSI (degrees) = %g',psi/dpr) 

sprintf(' PITCH THETA (degrees) = %g’,theta/dpr) 

sprintf('ROLL PHI (degrees) = %g’,phi/dpr) 
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